import java.awt.Color;
import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;
import java.util.LinkedList;
import java.util.Random;

import javax.swing.JFrame;

import ChartDirector.ChartViewer;

public class Swarm {

	static LinkedList<Particle> swarm = null;
	static LinkedList<Double> fitnessList = null;
	static LinkedList<Double> pBest = null;
	static LinkedList<Position> pBestLoc = null;
	

	static int SWARM_SIZE = 30;
	static int DIMENSION = 2;
	static int MAX_ITERATION = 200;
	static double C1 = 2.0;
	static double C2 = 2.0;
	static double W_UP = 1.0;
	static double W_LO = 0.0;
	
	static private Double gBest;
	static private Position gBestLoc;
	//isMaximum will define if the PSO will find the maximum solutions or minimum solutions
	private static boolean isMaximum = false;
	// funtion1 define which funtion we'll use
	public static boolean funtion1 = false;

	private static JFrame frame;
	private static ChartViewer viewer;
	
	/**
	 * Create all the particle in the swarm with the following constraints 
	 * of the function:
	 * 
	 *  Funcion 1
	 * f(x,y) = (2.8125-x+xy^4)^2+(2.25-x+xy^2)^2+(1.5-x+xy)^2
	 * 
	 * 1<=x<=4;
	 * -1<=y<=1;
	 * 
	 *  Funcion 2
	 *  Six-hump camel back function 
	 * f (x,y) = ( 4 - 2.1 x2 +  x4/3) x2 + xy + ( - 4  +  4y2) y2;    
	 * 
     *	-3 <= x <= 3, -2 <= y <= 2.
	 *	Global minimum
     *	f (x, y) =  - 1.0316;  (x, y)  =  (-0.0898,0.7126), (0.0898,-0.7126). 
	 */
	private static void initializeSwarm(Vector vector) {
		Particle p;
		Random generator = new Random();
		swarm = new LinkedList<Particle>();

		for (int i = 0; i < SWARM_SIZE; i++) {
			p = new Particle();
			double posX;
			double posY;
			double velX;
			double velY;
			if(funtion1){
				posX = generator.nextDouble() * 3.0 + 1.0;
				posY = generator.nextDouble() * 2.0 - 1.0;
				
				velX = generator.nextDouble() * 2.0 - 1.0;
				velY = generator.nextDouble() * 2.0 - 1.0;

			}else{
				posX = generator.nextDouble() * 6.0 - 3.0;
				posY = generator.nextDouble() * 4.0 - 2.0;
				
				velX = generator.nextDouble() * 2.0 - 1.0;
				velY = generator.nextDouble() * 2.0 - 1.0;
			}
			
			// Set the position and velocity of the particle adding it in the swarm 
			p.setLocation(new Position(posX, posY));
			p.setVelocity(new Velocity(velX, velY));
			swarm.add(p);
			
			// Set the position of the particle in the chart
			vector.setDataX(posX, i);
			vector.setDataY(posY, i);
			vector.setDataR(Math.sqrt((posX - velX)*(posX - velX) + (posY - velY)*(posY - velY)), i);
			vector.setDataA(Math.atan((posY - velY)*(posX - velX)), i);
		}
		
		//Create and set up the main window
        frame = new JFrame("Particle Swarm Optimization");
        frame.addWindowListener(new WindowAdapter() {
            public void windowClosing(WindowEvent e) {System.exit(0);} });
        frame.getContentPane().setBackground(Color.white);
        
        // Create the chart and put them in the content pane
        viewer = new ChartViewer();
        vector.createChart(viewer, 0);
        frame.getContentPane().add(viewer);
        
        // Display the window
        frame.pack();
        frame.setVisible(true);
	}

	public static void execute(Vector vector) {
		Random generator = new Random();
		initializeSwarm(vector);
		
		//evolutionaryStateEstimation();
		int currIteration = 0;
		double w;

		while (currIteration < MAX_ITERATION) {
			// calculate corresponding f(i,t) corresponding to location x(i,t)
			fitnessList=calculateAllFitness();

			// update pBest
			if (currIteration == 0) {
				for (int i = 0; i < SWARM_SIZE; i++) {
					pBest.add(i,fitnessList.get(i)); // Add fitness
					pBestLoc.add(i,swarm.get(i).getLocation()); // Add location of particle
				}
			} else {
				for (int i = 0; i < SWARM_SIZE; i++) {
					/*
					 * pBest list have all best value of each particle
					 */
					if(isMaximum){
						if (fitnessList.get(i) > pBest.get(i)) {
							pBest.remove(i); 
							pBestLoc.remove(i);
							pBest.add(i, fitnessList.get(i));
							pBestLoc.add(i, swarm.get(i).getLocation());
						}
					}else
						if (fitnessList.get(i) < pBest.get(i)) {
							pBest.remove(i); 
							pBestLoc.remove(i);
							pBest.add(i, fitnessList.get(i));
							pBestLoc.add(i, swarm.get(i).getLocation());
						}
					
				}
			}

			int bestIndex = getBestParticle();
			// update gBest
			// isMaximum will branch if we need the maximum or the minimum
			if(isMaximum){
				if (currIteration == 0 || pBest.get(bestIndex) > gBest) {
					gBest = fitnessList.get(bestIndex);
					gBestLoc = swarm.get(bestIndex).getLocation();
				}
			}else
				if (currIteration == 0 || pBest.get(bestIndex) < gBest) {
					gBest = fitnessList.get(bestIndex);
					gBestLoc = swarm.get(bestIndex).getLocation();
				}

			// w= inertia factor
			// this implementation considers the big value of inertia factor in the begin of iteration
			// and in the final of iteration a little inertia factor
			// PS: in the final the particles should be near by solution.
			w = W_UP - (((double) currIteration) / MAX_ITERATION) * (W_UP - W_LO);

			for (int i = 0; i < SWARM_SIZE; i++) {
				// update particle Velocity
				double r1 = generator.nextDouble();
				double r2 = generator.nextDouble();
				double lx = swarm.get(i).getLocation().getX();
				double ly = swarm.get(i).getLocation().getY();
				double vx = swarm.get(i).getVelocity().getX();
				double vy = swarm.get(i).getVelocity().getY();
				double pBestX = pBestLoc.get(i).getX();
				double pBestY = pBestLoc.get(i).getY();
				double gBestX = gBestLoc.getX();
				double gBestY = gBestLoc.getY();

				double newVelX = (w * vx)  // w = inertia factor
						+ (r1 * C1) * (pBestX - lx) // C1= cognitive weight
						+ (r2 * C2) * (gBestX - lx); // C2= social weight
				double newVelY = (w * vy) 
						+ (r1 * C1) * (pBestY - ly)
						+ (r2 * C2) * (gBestY - ly);
				swarm.get(i).setVelocity(new Velocity(newVelX, newVelY));

				// update particle Location
				double newPosX = lx + newVelX;
				double newPosY = ly + newVelY;
				swarm.get(i).setLocation(new Position(newPosX, newPosY));
				
				// update the size and angle of the current vector
				vector.setDataR(Math.sqrt((newPosX - newVelX)*(newPosX - newVelX) + (newPosY - newVelY)*(newPosY - newVelY)), i);
				vector.setDataA(Math.atan(newPosY - newVelY)*(newPosX - newVelX), i);
				
				// update the vector position
				vector.setDataX(newPosX, i);
				vector.setDataY(newPosY, i);
				
				// Create the chart and put them in the content pane
				vector.createChart(viewer, 0);
				frame.pack();
				frame.setVisible(true);
				System.out.println("X: " + newPosX + " Y: " + newPosY);
			}
			System.out.println("Interacao=" + currIteration + " Gbest : " + gBest);
			currIteration++;
		}
	}
	//Get the global best particle index of the Swarm in the current iteration
	private static int getBestParticle() {

		double Gbest=10;//Valor qualquer
		int GbestParticle=0;
		for(int i=0;i<SWARM_SIZE;i++){
			if(isMaximum){
				if(pBest.get(i).doubleValue() > Gbest){
					Gbest=pBest.get(i).doubleValue();
					GbestParticle=i;
				}

			}else
				if(pBest.get(i).doubleValue() < Gbest){
					Gbest=pBest.get(i).doubleValue();
					GbestParticle=i;
				}
		}
		return GbestParticle;
	}

	//Calculate the current fitness of all particle
	private static LinkedList<Double> calculateAllFitness() {
		LinkedList<Double> ret=new LinkedList<Double>();
		for(int i=0;i<SWARM_SIZE;i++){
			ret.add(swarm.get(i).getFitness());
		}
		return ret;
	}

	public static void main(String[] args) {
		//Instantiate an instance of this demo module
        Vector demo = new Vector(SWARM_SIZE);
        pBest = new LinkedList<Double>();
		pBestLoc = new LinkedList<Position>();
		execute(demo);
	}
}
